
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       gp_arming.c
  * @author     baiyang
  * @date       2021-8-18
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "gp_arming.h"

#include <rtconfig.h>

#include <copter/fms.h>
#include <parameter/param.h>

#if HAL_WITH_IO_MCU
#include <iomcu/gp_iomcu.h>
#endif

#if CONFIG_HAL_BOARD != HAL_BOARD_SITL_WIN
#include <sensor_imu/sensor_imu.h>
#include <sensor_gps/sensor_gps.h>
#include <sensor_baro/sensor_baro.h>
#include <sensor_compass/sensor_compass.h>
#endif

#include <board_config/borad_config.h>
#include <srv_channel/srv_channel.h>
/*-----------------------------------macro------------------------------------*/
#define AP_ARMING_COMPASS_MAGFIELD_EXPECTED 530
#define AP_ARMING_COMPASS_MAGFIELD_MIN  185     // 0.35 * 530 milligauss
#define AP_ARMING_COMPASS_MAGFIELD_MAX  875     // 1.65 * 530 milligauss
#define AP_ARMING_BOARD_VOLTAGE_MAX     5.8f
#define AP_ARMING_ACCEL_ERROR_THRESHOLD 0.75f
#define AP_ARMING_AHRS_GPS_ERROR_MAX    10      // accept up to 10m difference between AHRS and GPS

#define ARMING_RUDDER_DEFAULT         ARMING_ARMDISARM

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void bind_function(gp_arming_t arming, gp_arming_ops_t ops);
static bool arming_rc_arm_checks(gp_arming_t arming, ArmingMethod method);
static bool arming_manual_transmitter_checks(gp_arming_t arming, bool report);
static bool hardware_safety_check(gp_arming_t arming, bool report);
static bool compass_checks(gp_arming_t arming, bool report);
static bool servo_checks(gp_arming_t arming, bool report);
/*----------------------------------variable----------------------------------*/
static struct gp_arming_ops arming_ops = {.arm = arm,
                                   .disarm = disarm,
                                   .pre_arm_checks = pre_arm_checks,
                                   .arm_checks = arm_checks,
                                   .barometer_checks = barometer_checks,
                                   .ins_checks = ins_checks,
                                   .gps_checks = gps_checks,
                                   .board_voltage_checks = NULL,
                                   .rc_calibration_checks = rc_calibration_checks,
                                   .system_checks = NULL,
                                   .proximity_checks = NULL,
                                   .mandatory_checks = mandatory_checks};
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       构造函数
  * @param[in]   arming  
  * @param[out]  
  * @retval      
  * @note        
  */
void arming_ctor(gp_arming_t arming, gp_arming_ops_t ops)
{
    bind_function(arming, ops);

    param_link_variable(PARAM_ID(ARMING, ARMING_REQUIRE), &arming->require);
    param_link_variable(PARAM_ID(ARMING, ARMING_CHECK), &arming->checks_to_perform);
    param_link_variable(PARAM_ID(ARMING, ARMING_ACCTHRESH), &arming->accel_error_threshold);
    param_link_variable(PARAM_ID(ARMING, ARMING_RUDDER), &arming->_rudder_arming);
    param_link_variable(PARAM_ID(ARMING, ARMING_MIS_ITEMS), &arming->_required_mission_items);

    // method that was last used for disarm; invalid unless the
    // vehicle has been disarmed at least once.
    arming->_last_disarm_method = ARMING_CHECK_UNKNOWN;
}

static void bind_function(gp_arming_t arming, gp_arming_ops_t ops)
{
    arming->ops = &arming_ops;

    if (ops->arm == NULL) {
        ops->arm = arming->ops->arm;
    }

    if (ops->disarm == NULL) {
        ops->disarm = arming->ops->disarm;
    }

    if (ops->pre_arm_checks == NULL) {
        ops->pre_arm_checks = arming->ops->pre_arm_checks;
    }

    if (ops->arm_checks == NULL) {
        ops->arm_checks = arming->ops->arm_checks;
    }

    if (ops->barometer_checks == NULL) {
        ops->barometer_checks = arming->ops->barometer_checks;
    }

    if (ops->ins_checks == NULL) {
        ops->ins_checks = arming->ops->ins_checks;
    }

    if (ops->gps_checks == NULL) {
        ops->gps_checks = arming->ops->gps_checks;
    }

    if (ops->board_voltage_checks == NULL) {
        ops->board_voltage_checks = arming->ops->board_voltage_checks;
    }

    if (ops->rc_calibration_checks == NULL) {
        ops->rc_calibration_checks = arming->ops->rc_calibration_checks;
    }

    if (ops->system_checks == NULL) {
        ops->system_checks = arming->ops->system_checks;
    }

    if (ops->proximity_checks == NULL) {
        ops->proximity_checks = arming->ops->proximity_checks;
    }

    if (ops->mandatory_checks == NULL) {
        ops->mandatory_checks = arming->ops->mandatory_checks;
    }

    arming->ops = ops;
}

bool arming_is_armed(gp_arming_t arming)
{
    return (ArmingRequired)arming->require == ARMING_NO || arming->armed;
}

// mandatory checks that cannot be bypassed.  This function will only be called if ARMING_CHECK is zero or arming forced
bool mandatory_checks(gp_arming_t arming, bool report)
{
    return true;
}

//returns true if arming occurred successfully
bool arm(gp_arming_t arming, ArmingMethod method, const bool do_arming_checks)
{
    if (arming->armed) { //already armed
        return false;
    }

    if ((!do_arming_checks && arming_mandatory_checks(arming, true)) || (arming_pre_arm_checks(arming, true) && arming_arm_checks(arming, method))) {
        arming->armed = true;

        //Log_Write_Arm(!do_arming_checks, method); // note Log_Write_Armed takes forced not do_arming_checks

    } else {
        //AP::logger().arming_failure();
        arming->armed = false;
    }

    return arming->armed;
}

//returns true if disarming occurred successfully
bool disarm(gp_arming_t arming, ArmingMethod method, bool do_disarm_checks)
{
    if (!arming->armed) { // already disarmed
        return false;
    }
    arming->armed = false;
    arming->_last_disarm_method = method;

    //Log_Write_Disarm(method); // should be able to pass through force here?

    return true;
}

bool pre_arm_checks(gp_arming_t arming, bool report)
{
    if (arming->armed || arming->require == (uint8_t)ARMING_NO) {
        // if we are already armed or don't need any arming checks
        // then skip the checks
        return true;
    }

#if 0
    return hardware_safety_check(report)
        &  barometer_checks(report)
        &  ins_checks(report)
        &  compass_checks(report)
        &  gps_checks(report)
        &  battery_checks(report)
        &  logging_checks(report)
        &  manual_transmitter_checks(report)
        &  mission_checks(report)
        &  rangefinder_checks(report)
        &  servo_checks(report)
        &  board_voltage_checks(report)
        &  system_checks(report)
        &  can_checks(report)
        &  generator_checks(report)
        &  proximity_checks(report)
        &  camera_checks(report)
        &  osd_checks(report)
        &  fettec_checks(report)
        &  visodom_checks(report)
        &  aux_auth_checks(report)
        &  disarm_switch_checks(report)
        &  fence_checks(report);
#endif
    return arming_manual_transmitter_checks(arming, report)
        & arming_gps_checks(arming, report)
        & hardware_safety_check(arming, report)
        & arming_barometer_checks(arming, report)
        & arming_ins_checks(arming, report)
        & compass_checks(arming, report)
        & servo_checks(arming, report);
}

bool arm_checks(gp_arming_t arming, ArmingMethod method)
{
    if ((arming->checks_to_perform & ARMING_CHECK_ALL) ||
        (arming->checks_to_perform & ARMING_CHECK_RC)) {
        if (!arming_rc_arm_checks(arming, method)) {
            return false;
        }
    }

    // note that this will prepare AP_Logger to start logging
    // so should be the last check to be done before arming

    // Note also that we need to PrepForArming() regardless of whether
    // the arming check flag is set - disabling the arming check
    // should not stop logging from working.
#if 0
    AP_Logger *logger = AP_Logger::get_singleton();
    if (logger->logging_present()) {
        // If we're configured to log, prep it
        logger->PrepForArming();
        if (!logger->logging_started() &&
            ((checks_to_perform & ARMING_CHECK_ALL) ||
             (checks_to_perform & ARMING_CHECK_LOGGING))) {
            check_failed(ARMING_CHECK_LOGGING, true, "Logging not started");
            return false;
        }
    }
#endif

    return true;
}

bool arming_check_enabled(gp_arming_t arming, const ArmingChecks check)
{
    if (arming->checks_to_perform & ARMING_CHECK_ALL) {
        return true;
    }

    return (arming->checks_to_perform & check);
}

MAV_SEVERITY arming_check_severity(gp_arming_t arming, const ArmingChecks check)
{
    if (arming_check_enabled(arming, check)) {
        return MAV_SEVERITY_CRITICAL;
    }
    return MAV_SEVERITY_DEBUG; // technically should be NOTICE, but will annoy users at that level
}

void arming_check_failed(gp_arming_t arming, bool report, const char *fmt, ...)
{
    if (!report) {
        return;
    }

    char taggedfmt[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
    snprintf(taggedfmt, sizeof(taggedfmt), "PreArm: %s", fmt);
    va_list arg_list;
    va_start(arg_list, fmt);
    mavproxy_send_textv(MAV_SEVERITY_CRITICAL, taggedfmt, arg_list);
    va_end(arg_list);
}

void arming_check_failed2(gp_arming_t arming, const ArmingChecks  check, bool report, const char *fmt, ...)
{
    if (!report) {
        return;
    }
    char taggedfmt[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
    snprintf(taggedfmt, sizeof(taggedfmt), "PreArm: %s", fmt);
    MAV_SEVERITY severity = arming_check_severity(arming, check);
    va_list arg_list;
    va_start(arg_list, fmt);
    mavproxy_send_textv(severity, taggedfmt, arg_list);
    va_end(arg_list);
}

bool rc_calibration_checks(gp_arming_t arming, bool report)
{
    bool check_passed = true;
    const uint8_t num_channels = RCs_get_valid_channel_count();
    for (uint8_t i = 0; i < RC_INPUT_MAX_CHANNELS; i++) {
        RC_HandleTypeDef *c = RCs_get_channel(i);
        if (c == NULL) {
            continue;
        }
        if (i >= num_channels && !RC_has_override(c)) {
            continue;
        }
        const uint16_t trim = c->_radio_trim;
        if (c->_radio_min > trim) {
            arming_check_failed2(arming, ARMING_CHECK_RC, report, "RC%d_MIN is greater than RC%d_TRIM", i + 1, i + 1);
            check_passed = false;
        }
        if (c->_radio_max < trim) {
            arming_check_failed2(arming, ARMING_CHECK_RC, report, "RC%d_MAX is less than RC%d_TRIM", i + 1, i + 1);
            check_passed = false;
        }
    }

    return check_passed;
}

// Copter and sub share the same RC input limits
// Copter checks that min and max have been configured by default, Sub does not
bool arming_rc_checks_copter_sub(gp_arming_t arming, const bool display_failure)
{
    const RC_HandleTypeDef *channels[] = {
        fms.channel_roll,
        fms.channel_pitch,
        fms.channel_throttle,
        fms.channel_yaw
    };
        
    // set rc-checks to success if RC checks are disabled
    if ((arming->checks_to_perform != ARMING_CHECK_ALL) && !(arming->checks_to_perform & ARMING_CHECK_RC)) {
        return true;
    }

    bool ret = true;

    const char *channel_names[] = { "Roll", "Pitch", "Throttle", "Yaw" };

    for (uint8_t i=0; i<ARRAY_SIZE(channel_names);i++) {
        const RC_HandleTypeDef *channel = channels[i];
        const char *channel_name = channel_names[i];
        // check if radio has been calibrated
        if (channel->_radio_min > RC_CALIB_MIN_LIMIT_PWM) {
            arming_check_failed2(arming, ARMING_CHECK_RC, display_failure, "%s radio min too high", channel_name);
            ret = false;
        }
        if (channel->_radio_max < RC_CALIB_MAX_LIMIT_PWM) {
            arming_check_failed2(arming, ARMING_CHECK_RC, display_failure, "%s radio max too low", channel_name);
            ret = false;
        }
        bool fail = true;
        if (i == 2) {
            // skip checking trim for throttle as older code did not check it
            fail = false;
        }
        if (channel->_radio_trim < channel->_radio_min) {
            arming_check_failed2(arming, ARMING_CHECK_RC, display_failure, "%s radio trim below min", channel_name);
            if (fail) {
                ret = false;
            }
        }
        if (channel->_radio_trim > channel->_radio_max) {
            arming_check_failed2(arming, ARMING_CHECK_RC, display_failure, "%s radio trim above max", channel_name);
            if (fail) {
                ret = false;
            }
        }
    }
    return ret;
}

bool gps_checks(gp_arming_t arming, bool report)
{
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL_WIN
    if ((arming->checks_to_perform & ARMING_CHECK_ALL) || (arming->checks_to_perform & ARMING_CHECK_GPS)) {

        // Any failure messages from GPS backends
            char failure_msg[50] = {};
            if (!sensor_gps_backends_healthy(failure_msg, ARRAY_SIZE(failure_msg))) {
                if (failure_msg[0] != '\0') {
                    arming_check_failed2(arming, ARMING_CHECK_GPS, report, "%s", failure_msg);
                }
                return false;
        }

        for (uint8_t i = 0; i < sensor_gps_num_sensors(); i++) {
#if defined(GPS_BLENDED_INSTANCE)
            if ((i != GPS_BLENDED_INSTANCE) &&
#else
            if (
#endif
                    (sensor_gps_get_type(i) == GPS_TYPE_NONE)) {
                if (gps.primary_instance == i) {
                    arming_check_failed2(arming, ARMING_CHECK_GPS, report, "GPS %i: primary but TYPE 0", i+1);
                    return false;
                }
                continue;
            }

            //GPS OK?
            if (sensor_gps_status2(i) < GPS_OK_FIX_3D) {
                arming_check_failed2(arming, ARMING_CHECK_GPS, report, "GPS %i: Bad fix", i+1);
                return false;
            }

            //GPS update rate acceptable
            if (!sensor_gps_is_healthy(i)) {
                arming_check_failed2(arming, ARMING_CHECK_GPS, report, "GPS %i: not healthy", i+1);
                return false;
            }
        }

        if (!ahrs_home_is_set(fms.ahrs)) {
            arming_check_failed2(arming, ARMING_CHECK_GPS, report, "GPS: waiting for home");
            return false;
        }

        // check GPSs are within 50m of each other and that blending is healthy
        float distance_m;
        if (!sensor_gps_all_consistent(&distance_m)) {
            arming_check_failed2(arming, ARMING_CHECK_GPS, report, "GPS positions differ by %4.1fm",
                         (double)distance_m);
            return false;
        }
        if (!sensor_gps_blend_health_check()) {
            arming_check_failed2(arming, ARMING_CHECK_GPS, report, "GPS blending unhealthy");
            return false;
        }

        // check AHRS and GPS are within 10m of each other
        const Location* gps_loc = sensor_gps_location();
        Location ahrs_loc;
        if (ahrs_get_position(fms.ahrs, &ahrs_loc)) {
            const float distance = location_get_distance((Location* )gps_loc, &ahrs_loc);
            if (distance > AP_ARMING_AHRS_GPS_ERROR_MAX) {
                arming_check_failed2(arming, ARMING_CHECK_GPS, report, "GPS and AHRS differ by %4.1fm", (double)distance);
                return false;
            }
        }
    }

    if ((arming->checks_to_perform & ARMING_CHECK_ALL) || (arming->checks_to_perform & ARMING_CHECK_GPS_CONFIG)) {
        uint8_t first_unconfigured;
        if (sensor_gps_first_unconfigured_gps(&first_unconfigured)) {
            arming_check_failed2(arming, ARMING_CHECK_GPS_CONFIG,
                         report,
                         "GPS %d failing configuration checks",
                         first_unconfigured + 1);
            if (report) {
                sensor_gps_broadcast_first_configuration_failure_reason();
            }
            return false;
        }
    }
#endif

    return true;
}

bool barometer_checks(gp_arming_t arming, bool report)
{
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL_WIN
    if ((arming->checks_to_perform & ARMING_CHECK_ALL) ||
        (arming->checks_to_perform & ARMING_CHECK_BARO)) {
        if (!sensor_baro_all_healthy()) {
            arming_check_failed2(arming, ARMING_CHECK_BARO, report, "Barometer not healthy");
            return false;
        }
    }
#endif

    return true;
}

bool ins_checks(gp_arming_t arming, bool report)
{
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL_WIN
    if ((arming->checks_to_perform & ARMING_CHECK_ALL) ||
        (arming->checks_to_perform & ARMING_CHECK_INS)) {
        if (!sensor_imu_get_gyro_health_all()) {
            arming_check_failed2(arming, ARMING_CHECK_INS, report, "Gyros not healthy");
            return false;
        }
        if (!sensor_imu_gyro_calibrated_ok_all()) {
            arming_check_failed2(arming, ARMING_CHECK_INS, report, "Gyros not calibrated");
            return false;
        }
        if (!sensor_imu_get_accel_health_all()) {
            arming_check_failed2(arming, ARMING_CHECK_INS, report, "Accels not healthy");
            return false;
        }
#if 0
        if (!ins.accel_calibrated_ok_all()) {
            check_failed(ARMING_CHECK_INS, report, "3D Accel calibration needed");
            return false;
        }
        
        //check if accelerometers have calibrated and require reboot
        if (ins.accel_cal_requires_reboot()) {
            check_failed(ARMING_CHECK_INS, report, "Accels calibrated requires reboot");
            return false;
        }

        // check all accelerometers point in roughly same direction
        if (!ins_accels_consistent(ins)) {
            check_failed(ARMING_CHECK_INS, report, "Accels inconsistent");
            return false;
        }

        // check all gyros are giving consistent readings
        if (!ins_gyros_consistent(ins)) {
            check_failed(ARMING_CHECK_INS, report, "Gyros inconsistent");
            return false;
        }

        // no arming while doing temp cal
        if (sensor_imu_temperature_cal_running()) {
            check_failed(ARMING_CHECK_INS, report, "temperature cal running");
            return false;
        }

        // check AHRS attitudes are consistent
        char failure_msg[50] = {};
        if (!AP::ahrs().attitudes_consistent(failure_msg, ARRAY_SIZE(failure_msg))) {
            check_failed(ARMING_CHECK_INS, report, "%s", failure_msg);
            return false;
        }
#endif
    }
#endif

    return true;
}

static bool arming_rc_arm_checks(gp_arming_t arming, ArmingMethod method)
{
    // don't check the trims if we are in a failsafe
    if (!RCs_has_valid_input()) {
        return true;
    }

    // only check if we've received some form of input within the last second
    // this is a protection against a vehicle having never enabled an input
    uint32_t last_input_ms = Rcs.last_update_ms;
    if ((last_input_ms == 0) || ((time_millis() - last_input_ms) > 1000)) {
        return true;
    }

    bool check_passed = true;
    // ensure all rc channels have different functions
    if (RCs_duplicate_options_exist()) {
        arming_check_failed2(arming, ARMING_CHECK_PARAMETERS, true, "Duplicate Aux Switch Options");
        check_passed = false;
    }
    if (RCs_flight_mode_channel_conflicts_with_rc_option()) {
        arming_check_failed2(arming, ARMING_CHECK_PARAMETERS, true, "Mode channel and RC%d_OPTION conflict", RCs_flight_mode_channel_number());
        check_passed = false;
    }
    
    return check_passed;
}

static bool arming_manual_transmitter_checks(gp_arming_t arming, bool report)
{
    if ((arming->checks_to_perform & ARMING_CHECK_ALL) ||
        (arming->checks_to_perform & ARMING_CHECK_RC)) {

        if (fms.failsafe.radio) {
            arming_check_failed2(arming, ARMING_CHECK_RC, report, "Radio failsafe on");
            return false;
        }

        if (!arming_rc_calibration_checks(arming, report)) {
            return false;
        }
    }

    return true;
}

static bool hardware_safety_check(gp_arming_t arming, bool report)
{
    if ((arming->checks_to_perform & ARMING_CHECK_ALL) ||
        (arming->checks_to_perform & ARMING_CHECK_SWITCH)) {

      // check if safety switch has been pushed
      if (brd_safety_switch_state() == SAFETY_DISARMED) {
          arming_check_failed2(arming, ARMING_CHECK_SWITCH, report, "Hardware safety switch");
          return false;
      }
    }

    return true;
}

static bool compass_checks(gp_arming_t arming, bool report)
{
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL_WIN
    // check if compass is calibrating
    if (sensor_compass_is_calibrating()) {
        arming_check_failed(arming, report, "Compass calibration running");
        return false;
    }

    // check if compass has calibrated and requires reboot
    if (sensor_compass_cal_requires_reboot()) {
        arming_check_failed(arming, report, "Compass calibrated requires reboot");
        return false;
    }

    if ((arming->checks_to_perform) & ARMING_CHECK_ALL ||
        (arming->checks_to_perform) & ARMING_CHECK_COMPASS) {

        // avoid Compass::use_for_yaw(void) as it implicitly calls healthy() which can
        // incorrectly skip the remaining checks, pass the primary instance directly
        if (!sensor_compass_use_for_yaw2(0)) {
            // compass use is disabled
            return true;
        }

        extern bool sensor_compass_healthy(void);
        if (!sensor_compass_healthy()) {
            arming_check_failed2(arming, ARMING_CHECK_COMPASS, report, "Compass not healthy");
            return false;
        }
        // check compass learning is on or offsets have been set
        // check compass offsets have been set if learning is off
        // copter and blimp always require configured compasses
        if (!sensor_compass_learn_offsets_enabled())
        {
            char failure_msg[50] = {};
            if (!sensor_compass_configured2(failure_msg, ARRAY_SIZE(failure_msg))) {
                arming_check_failed2(arming, ARMING_CHECK_COMPASS, report, "%s", failure_msg);
                return false;
            }
        }

        // check for unreasonable compass offsets
        const Vector3f_t* offsets = sensor_compass_get_offsets();
        if (vec3_length(offsets) > sensor_compass_get_offsets_max()) {
            arming_check_failed2(arming, ARMING_CHECK_COMPASS, report, "Compass offsets too high");
            return false;
        }

        // check for unreasonable mag field length
        const float mag_field = vec3_length(sensor_compass_get_field());
        if (mag_field > AP_ARMING_COMPASS_MAGFIELD_MAX || mag_field < AP_ARMING_COMPASS_MAGFIELD_MIN) {
            arming_check_failed2(arming, ARMING_CHECK_COMPASS, report, "Check mag field: %4.0f, max %d, min %d", (double)mag_field, AP_ARMING_COMPASS_MAGFIELD_MAX, AP_ARMING_COMPASS_MAGFIELD_MIN);
            return false;
        }

        // check all compasses point in roughly same direction
        if (!sensor_compass_consistent()) {
            arming_check_failed2(arming, ARMING_CHECK_COMPASS, report, "Compasses inconsistent");
            return false;
        }
    }
#endif

    return true;
}

static bool servo_checks(gp_arming_t arming, bool report)
{
#if NUM_SERVO_CHANNELS
    bool check_passed = true;
    for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
        const srv_channel *c = &srvs.channels[i];
        if (c == NULL || c->function == k_none) {
            continue;
        }

        const uint16_t trim = c->servo_trim;
        if (c->servo_min > trim) {
            arming_check_failed(arming, report, "SERVO%d_MIN is greater than SERVO%d_TRIM", i + 1, i + 1);
            check_passed = false;
        }
        if (c->servo_max < trim) {
            arming_check_failed(arming, report, "SERVO%d_MAX is less than SERVO%d_TRIM", i + 1, i + 1);
            check_passed = false;
        }
    }

#if HAL_WITH_IO_MCU
    if (!iomcu_healthy()) {
        arming_check_failed(arming, report, "IOMCU is unhealthy");
        check_passed = false;
    }
#endif

    return check_passed;
#else
    return false;
#endif
}

/*------------------------------------test------------------------------------*/


